from rplidar import RPLidar
import numpy as np
import time
import matplotlib.pyplot as plt

plt.ion()


class Mylidar:
    def __init__(self):
        self.lidar = RPLidar(port='COM4', timeout=5)
        
        self.min_x = -6
        self.max_x = 6
        self.x = []
        self.y = []
    
    def get_data(self):
        # info = lidar.get_info()
        # 三个数据 qulity angle dis
        for i, scan in enumerate(self.lidar.iter_scans()):
            yield scan
    
    def stop(self):
        
        self.lidar._serial_port.write(bytes([0xA5, 0x25]))
        # self.lidar._serial_port.write(bytes([0xA5, 0x20]))
        self.lidar.stop()
        self.lidar.stop_motor()
        self.lidar.disconnect()
        self.lidar._serial_port.close()

    def reset(self):
        self.lidar._serial_port.write(bytes([0xA5, 0x40]))
    
    def on_launch(self):
        self.figure, self.ax = plt.subplots()
        self.lines, = self.ax.plot([], [], 'o')
        # Autoscale on unknown axis and known lims on the other
        self.ax.set_autoscaley_on(True)
        self.ax.set_xlim(self.min_x, self.max_x)
        # Other stuff
        self.ax.grid()
    
    def on_running(self, xdata, ydata):
        # Update data (with the new _and_ the old points)
        self.x = self.x + list(xdata)
        self.y = self.y + list(ydata)
        self.lines.set_xdata(xdata)
        self.lines.set_ydata(ydata)
        # Need both of these in order to rescale
        self.ax.relim()
        self.ax.autoscale_view()
        # We need to draw *and* flush
        self.figure.canvas.draw()
        self.figure.canvas.flush_events()
        if len(self.x) > 5000:
            print("clear cache")
            self.x = []
            self.y = []
    
    def vis(self):
        self.on_launch()
        for data in self.get_data():
            x, y = self.process(data)
            self.on_running(x, y)

    def process(self, d):
        d1 = np.array(d)
        # qulity14 long 8000 ,short 200
        d2 = d1[(d1[:, 0] > 14) & (d1[:, 2] < 6000) & (d1[:, 2] > 200)]
        angle = d2[:, 1] * np.pi / 180
        dis = d2[:, 2] / 1000
        # 这样设置，尖头就是上方，和可视化一直
        y = -np.cos(angle) * dis
        x = np.sin(angle) * dis
        return x, y


if __name__ == '__main__':
    d = Mylidar()
    d.stop()
